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u.  abstract  {MtMimumioowo'di)  This  paper  describes  a  systems  approach  to  inproving  the  per¬ 
formance  of  a  telerobotic  testbed.  The  testbed  is  the  Force-Reflecting  Interfaces  to 
Telemanipulators  Testinq  System  (FITTS) ,  at  the  Harry  G.  Armstrong  Aerospace  Medical 
Research  Laboratory.  First,  the  testbed  hardware  is  described,  along  with  an  overview 
of  the  system’s  ccrmunications  paths.  Next,  the  previously-determined  forward  kine¬ 
matics  for  the  MBA  exoskeleton  are  outlined.  Then  the  optimization  of  these  kinemati< 
equations  is  given.  The  paper  also  details  efficient  forward  and  inverse  kinerutic 
solutions  for  the  Merlin  industrial  robots,  using  the  method  of  wrist  partitioning. 
The  utility  of  these  solutions  in  toto  is  that  the  interfacing  of  the  two  systems, 
given  sufficient  carnunications  bandwidth  between  the  MEW  exoskeleton  and  the  control 
ocmputer,  can  new  achieve  the  4  ms  oenpute  time  attainable  by  the  Merlin  hardware. 
This  optimization  puts  the  FITTS  hardware  az  a  milestone  stage  of  completion,  as  the 
system  is  now  capable  of  operating  at  its  peak  as  a  unilateral  telerobotic  testbed. 
Finally,  future  steps  to  extend  FITTS  for  force  reflection  research  are  specified. 


H.  WHCI  CODI 


S'.i*d«-d  *(>•"'  2  t 

Wl  .!,  • 

m  *w 


20  UMlTATtO*  01  AISTAACT 

UL 


14.  SUBJECT  TEAMS 

Tuleoperation;  Robotics;  Remote  Control;  Human  Factors; 


Force  Reflection 


17.  SECURITY  CLASSlHCATtDN 
0»  REPORT 

Unclassified 


II  SECURITY  CLASSIFICATION 
Of  THIS  P AGI 

Unclassified 


!t.  SECURITY  CLASS* KAVtON 
Of  AISTAACT 

Jnclassified 


IS.  NUMBER  Of  p »0IS 

17 


NSN  7*40-01  710  5S00 


Contents 

1  Introduction  2 

2  MBA  Exoskeleton  Forward  Kinematics  4 

3  Merlin  Robot  Kinematics  9 

3.1  Position .  g 

3.2  Orientation .  10 

4  Conclusions  11 


X  Introduction 


At  the  Harry  G.  Armstrong  Aerospace  Medical  Research  Laboratory,  the  Human  Sensory  Feedback 
team  of  the  Biological  Acoustics  Branch  is  investigating  technologies  for  intuitive  human  control  of, 
and  sensory  feedback  from,  teleoperated  robotic  devices.  To  this  end  the  Force- Reflecting  Interfaces 
to  Telemanipulators  Testing  System  (FITTS)  has  been  established  [5].  This  system  is  currently 
undergoing  refinement  before  its  use  as  a  dedicated  testbed  for  evaluating  man- machine  interfaces 
to  teleoperators. 

The  goal  of  the  FITTS  system  is  to  investigate  human  interfaces  to  telerobotic  systems.  The 
first  interface  to  be  evaluated  by  FITTS  is  the  MBA  Exoskeleton,  shown  in  Figure  l.  This  seven 
degree- of- freedom  (DOF),  unilateral  device  is  being  used  to  establish  performance  baselines  for 
worst-case  telerobotic  performance.  The  optimal  baselines,  of  course,  will  be  human  hands-on  task 
completion,  although  it  is  recognized  that  synergism  between  the  operator  and  the  robots  may 
eventually  surpass  this  now-optimal  baseline. 


Figure  1:  Seven  DOF  MBA  Exoskeleton. 

The  slave  robot*  used  in  FITTS  are  the  6  DOF  American  Robot  Corporation  Merlin  robots 
shown  in  Figure  2.  The  testbed  contains  a  left-right  pair  of  Merlins,  although  only  the  left- arm 
Merlin  is  currently  operating  at  high  speed.  Each  Merlin  has  a  50  lb  payload,  and  can  move  the 
end  elector  at  5  throughout  the  workspace.  The  left-arm  Merlin  has  beeu  equipped  with  the 
optional  High  Speed  Host  Interface  (HSH1),  which  allows  the  control  computer  to  update  desired 
position  or  velocity  commands  at  250  Hz  via  shared-memory  window. 

hi  addition  to  the  MBA  Exoskeleton  end  the  Merlin  robots,  FITTS  contains  a  peg-into-hole 
taskboard  developed  jointly  by  the  Naval  Oceans  Systems  Center  -  Hawaii,  and  AAMRL  (8).  This 
taskboard  is  particularly  well- suited  to  measuring  task  performance  using  a  Fitts’  Law  paradigm 
[4].  The  peg-into-hole  tasks  available  have  indices  of  difficulty  (ID)  ranging  from  6  to  12,  with 
the  tasks  of  higher  ID  resulting  from  closer  tolerances  between  the  pegs  and  the  holes  or  larger 
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amplitudes  of  movement.  Two  preliminary  studies  have  been  completed  at  AAMRX  using  this 
subsystem  of  FITTS  [7]  [6]  to  measure  the  task  performance  degradation  caused  by  the  MBA 
Exoskeleton,  exclusive  of  any  slave  robotic  systems. 


Figure  2:  Six  DOF  Merlin  Industrial  Robots. 


The  interactions  between  the  components  of  the  FITTS  testbed  are  shown  in  Figure  3.  Joint 
angle  data  flows  serially  from  the  MBA  Exoskeleton  to  the  Compaq  computer,  which  then  calculates 
the  endpoint  position  of  the  exoskeleton.  The  Compaq  next  computes  the  inverse  kinematics  of  the 
Merlin  robot,  and  feeds  the  joint  position  information  to  the  robot’s  controller  via  shared  RAM. 
The  Merlin  controller  internally  generates  a  trajectory  to  accomplish  this  motion. 


Communication 

Physical 

Interaction 


Figure  3:  FITTS  subsystems  and  their  interactions. 

This  paper  describes  recent  work  to  improve  the  performance  of  the  FITTS  system,  preparing 
it  first  f'"  unilateral,  then  for  bilateral  teleoperation.  Specifically,  the  steps  taken  to  optimise  the 
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kinematics  code  for  the  MBA  Exoskeleton  are  presented.  Next,  the  Merlin  kinematics  solutions, 
using  the  method  of  wrist  partitioning,  are  given.  It  is  shown  that  these  new  algorithms  reduce  the 
computational  time  by  75  per  cent  from  the  previous  methods.  An  analysis  of  the  FITTS  system’s 
communication  paths  is  then  completed  to  determine  the  improvements  in  communication  rates 
needed  to  optimize  performance  for  unilateral  teleoperation.  Finally,  the  process  is  given  which 
will  allow  the  FITTS  testbed  to  evaluate  bilateral  man* machine  interfaces,  such  as  force-reflecting 
exoskeletons. 

2  MBA  Exoskeleton  Forward  Kinematics 
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Table  1:  D-ll  paramaters  for  the  MBA  exoskeleton,  left  arm. 


We  begin  with  the  beiiavitllarleuburg  (D-ll)  parameters  for  the  MBA  exoskeleton,  as  found 
by  Gary  Merrill  of  Systems  Research  Laboratories,  Inc.  He  assigned  twelve  coordinate  frames  for 
each  arm,  shown  in  Figure  4  aud  summarized  in  Table  1.  See  (7]  for  more  details. 

In  Table  1,0,  is  the  mappiug  from  z,,,  to  z,  about  the  z,„i  axis,  a,  is  the  mapping  from  z,  , 
to  :,  about  the  z,  axis,  ci,  is  the  distance  between  the  z,_j  and  z,  axes  along  r,,  d,  is  the  distance 
between  the  z,-,  and  z.  axes  along  r,_i,  L,  is  the  length  of  link  i,  in  is  the  measured  joint  angle 
in  frame  II,  and  the  addend  tan  '(■)  term  is  a  fixed  olfset  for  this  frame. 

The  forward  kinematics  (of  a  single  arm)  of  the  MBA  exoskeleton  had  been  calculated  on  line  by 
iteratively  loading  and  then  multiplying  the  twelve  transformation  matrices  corresponding  to  each 
of  these  frames.  The  C  code  that  performed  this  operation  required  768  multiplies,  576  additions. 
48  trigonometric  function  evaluations,  over  400  variable  assignments,  36  nested  for  loops,  and  12 
function  calls  (with  variable  declarations  initializations).  Each  iteration  of  this  code  look  11.2  ms 
of  compute  time  on  a  33  MHz  Compaq  80386  personal  computer  with  an  80387  math  coprocessor. 

In  order  to  improve  controller  bandwidth  —  and  performance  —  it  was  deemed  necessary  to 
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explicitly  formulate  the  forward  kinematics  of  the  MBA  exotkeleton  by  hand,  and  then  calculate 
only  these  simplified  kinematics  on-line.  The  twelve  transformation  matrices  were  obtained  from 
the  D-H  parameters  and  finally  reduced  to  three.  The  calculations  are  shown  below. 

The  general  form  of  the  transformation  matrix  is  as  given  in  [1): 

f  c $i  -ca;s0,  sa,s0,  a,-cfj  ' 

T  _  s 6i  ca,c $i  -ta,c$t  a,s0, 

‘  0  SQ,  CQ,  d,  '  ' 

V  0  0  0  1 

where  cz  and  sz  are  shorthand  for  cos  z  and  sin  z,  respectively. 

Substituting  each  of  the  D-H  parameters  into  the  appropriate  T,,  the  twelve  transformation 
matrices  are  easily  found  to  be: 
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Finally,  these  six  arc  reduced  to  only  three  transformation  matrices.  These  three  matrices  are 
shown  below,  using  the  notation 

r,-i  =  (2i) 

and  we  adopt  the  additional  convention  of  using  c,  and  s,  to  represent  cos0,  and  sin0,,  respec¬ 
tively: 


T, - 


7s.k  = 


e  ,c  ,  -s3  c3s,  a2  -  d.Sj 

CQ;1|C|  4-SQiSi  CQ^C.i  CO;»  )l|  -  CO^C  j  -  <S| 

■q^s  )C  i  —  ca-ji-i  sajCi  sa^sja,  +  co^ci  disa^c^  +  dy 
0  0  0  1 

/  c-iCrCn  -  S1»H  -c%s7  “C^c7s#  -  s\c*  dKc*s7  -  da%%  > 
»5C7CM-fCs#«  “tjl;  -fsC7*K  +  €*€*  d^SsST  +  di,u 


= 


-! 

»?c* 

~C; 

87Sk  dKC7  -i 

0 

0 

0  1 

< 

-S.jCn 

-dijSySji  +  QyCy 

»*» 

CyCjj 

cy*n 

+  OySy 

0 

“*ll 

C|| 

diyCi;  f  dy 

l  0 

0 

0 

1 

These  three  matrices  were  coded  i.i  the  C  programming  language  on  the  Compaq  computer. 
The  remaining  two  matrix  multiplications, 

r,  ,  =  T,.i r,  *  (25) 


Ti.u  S  (26) 

were  explicitly  written  in  terms  of  the  matrix  elements,  with  multiplications  by  0,  1,  and  •  1  removed 
by  hand.  This  technique,  though  sub-optimal,  is  significantly  more  efficient  than  the  previous 
method  of  loading  and  multiplying  each  matrix  using  nested  for  loops.  The  resulting  code  has 
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only  112  multiplications,  75  additions,  14  trigonometric  function  evaluations,  about  75  variable 
assignments,  no  for  loops,  and  no  function  calls;  its  cycle  time  is  0.7  ms  on  the  Compaq  (versus 
11.2  ms  for  the  previous  method). 

Further  optimisation  can  be  achieved  in  two  ways,  if  desired.  The  first  procedure  is  to  explic¬ 
itly  combine  the  three  matrices  Tt_j,  T5_g,  and  Tg_i2  off-line.  This  results  in  obtaining  the  final 
transformation  matrix,  T*j _i2»  without  requiring  any  interim  matrix  computations.  The  computa¬ 
tional  advantage  of  this  method  comes  from  reducing  the  number  of  interim  variable  assignments 
and  register  manipulations.  Unfortunately,  the  complexity  of  these  matrices  suggests  the  use  of  a 
symbolic  math  program  to  perform  these  calculations. 

The  second  optimisation  procedure  is  to  search  the  final  matrices  (or  matrix  if  the  previous  op¬ 
timisation  method  is  also  used)  for  all  common  factors.  Each  common  factor  should  be  computed 
once,  and  the  resultant  of  this  calculation  used  at  each  occurrence  of  the  factor.  This  process  will 
reduce  the  number  of  multiplications,  trigonometric  function  evaluations,  and  register  manipula¬ 
tions.  Although  this  process  can  often  save  a  significant  amount  of  time,  in  our  case  the  number 
of  common  factors  appears  to  be  small;  t^us  the  savings  would  not  be  very  great. 

3  Merlin  Robot  Kinematics 

Since  the  Merlin  robot  has  a  spherical  wrist,  its  kinematic  formulation  may  be  partitioned  into 
position  (first  three  DOF)  and  orientation  (last  three  DOF)  [9].  The  forward  and  inverse  equations 
for  position  of  the  Merlin  robot  were  found  using  the  geometric  approach  (3];  those  for  orientation 
are  more  easily  found  because  the  wrist  is  spherical  [10].  The  complete  solution  is  very  similar  to 
that  of  a  PUMA  560  robot. 

The  following  sections  relate  the  forward  and  inverse  solutions  for  the  Merlin  without  describing 
the  process  in  detail.  We  again  adopt  cr  and  sr  as  shorthand  for  cos*  and  sinr,  respectively,  and 
the  additional  convention  of  using  c,  and  s,  to  represent  c:.s0,  and  sin0,,  respectively.  In  the 
sections  which  fellow,  we  u»*  the  general  form  of  a  transformation  matrix 


"a 

*s 

<*, 

Pt  \ 

7  = 

ny 

n, 

3. 

a. 

Pv 

P, 

(27) 

o 

0 

0 

l  ) 

with  T\i  j  representing  the  ijtli  entry  of  this  mat 

rix, 

and  0  , 

representing  the  array  of  joint 

angles  for  the  Merlin,  beginning  with  the  base  and  working  towards  the  final  hand  roll. 
3,1  Position 

(liven  S  i  the  forward  equations  to  the  wrist  are  as  folio  as: 


r  ■  /;C0;  *  /|C0,  (28) 

pt  =■  -i/.st?,  ♦  r  iC0,  (29) 

f>v  =  d*cdj  ♦  r  (30) 

P;  =  f.'S 6t  T  / |S0j  (31) 
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where  d-y  —  12.0,  l,  -  17.38,  In  =  17.24  (units  arc  inches),  0\ j  is  measured  absolutely,  and  ry  is  the 
vector  from  the  base  coordinate  frame  to  the  first  wrist  coordinate  frame. 

The  inverse  equations  for  wrist  position,  given  px ,  pv,  and  are  the  following: 


ry 

=  \jvl+Pl-#2 

(32) 

ll  ~  l2  +  r\\  +  p'i 

c 

2 

(33) 

-  atan2(r|j,  d2)  —  atan2 (pTiPy) 

(34) 

0: , 

=  atan2(pz, ry)  -f  atan2(rj|  +  p]  -  c2,  c) 

(35) 

h 

=  atan2(p*  —  l^sO^r^  -  hc9^) 

(36) 

where  0;\  is  again  given  as  an  absolute  angle. 

In  implementing  the  above  solutions,  it  should  be  noted  that  the  Merlin  controller  gives  the 
signs  of  0-,  and  On  opposite  to  convention,  and  hence  this  must  be  accounted  for  by  negating  these 
two  angles  before  (forward)  and  after  (inverse)  the  above  calculations  are  performed. 


3.2  Orientation 

Before  listing  the  equations  for  the  orientation  of  the  Merlin  wrist,  we  first  define  the  following 
common  factors  in  these  equations: 


C2;i  =  COS (0;]  -  7r/2) 

(37) 

s23  —  sin(0;j  —  7T /2  ) 

(38) 

k\  —  c.|  CjC(j  -  s.|S(; 

(39) 

hi  =  s,|Cr,cfi  -f  c.jSg 

(40) 

k,\  —  s.,cr,sG  —  c.|  C(5 

(41) 

k.\  ~  c 2-.ik\  -  S23S5CG 

(42) 

kr,  ~  c.| c  15 S(j  T  S.,CG 

(43) 

kc>  ~  C23C.1S5  +  S23C5 

(44) 

With  these  common  factors  defined, 
to  the  following: 

the  forward  kinematics  equations  for  wrist  orientation 

simplify 

mm 

—  c  1  k  1  -f  s  1  k-2 

(45) 

T[1][0] 

=  s\k.t  -c\k2 

(46) 

J’|2][0] 

~  -S2:\k  1  -  c2;iSf,Cf; 

(47) 

T[0][1] 

=  c  1  ( - c 2:\kr>  *f  S2:,s.-, s(i )  -  s t  k:\ 

(48) 

nm 

~  s,(~c2:^r,  f  s2;jsr,s0)  +  c , k:\ 

(49) 

nm 

Sj.’jAjr,  -1-  C2;jSr,S(5 

(50) 

nm 

-C|  -  sjs  jSr, 

(51) 

rim 

—  ~  s  1  ko  T  c  1  s  |  S3 

(52) 

nm 

“  S23C48 r>  ™  c.2:icn 

(53) 
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The  inverse  equations  for  the  wrist  are  given  below.  We  will  show  each  angle’s  solution  in 
turn  after  defining  some  temporary  constants  for  each.  First  6\  and  9.\  must  be  found  with  the 
inverse  solution  for  wrist  position.  To  solve  for  wrist  orientation  we  also  need  C23  and  S23  (where 
the  subscript  2*  represents  62  +  0j)-  Note  that 

c23  =  cos(03-*/2)  (54) 

s 23  =  s»n(03  -  ir/2)  (55) 

because  of  the  absolute  measurement  of  #3,  and  because  92  has  an  initial  alignment  of  tt/2  from 
the  base  frame. 

For  6 Ay  we  have 


U  =  T(2![2]s23-T[0][2]c1C23-T[1][2]s1c2J  (56) 

h  =  T(l][2]c, -T[0][2]s,  (57) 

0X  =  atan2(<2,<i)  (58) 

For  0r„  we  have 

U  =  -3,[0](2]ClS23-Tfl)(2]s1S2:,-T!2][2]c2:1  (59) 

h  =  T[2](2]s2:ic  1  -  T[0)(2](c|C2.ic,  +  s, s  1)  -  T[l][2j(s, cj.jc ,  -  c,sj)  (60) 

0- ,  =  atan2(tv,ti)  (61) 

For  0fi,  we  have 

t,  =  T(0](0)(v5(ciC2:iCi  +  Sjs  j)  -  C|S*is.\)  (62) 

+  ril][0](e5(s,C2:»c«  -  c,si)  -  sisjjs.s)  -  T[2][0](s/:ic.|Cs  4-  C23S5)  (63) 

tj  =  T(2](0]S2;JS,  -  T[0][0](C|C2,S,  -  S|C,)  -  T\  l!(0](s,c2:,s !  +  c  1  c  i )  (64) 

9{ ;  =  atan2(<2i  ti )  (65) 


To  move  the  robot  wrist  to  9\ ,  tij,  realize  that  th<  Merlin  distinguishes  between  1°,  36T,  and 
-359°,  but  the  atan2  function  only  returns  values  in  the  range  of  ±180°.  To  account  for  this 
discrepancy,  a  section  of  C  code  was  edded  to  the  inverse  solution  immediately  following  the  com¬ 
putation  of  9\.  This  code  keeps  track  of  as  the  wrist  rolls  about  its  axis,  and  adds  ±180'  as  often 
as  necessary  to  prevent  a  “flip”  of  180°  when  moving  to  the  new  position.  The  con.  also  tracks  <?,,, 
and  uses  the  same  technique  to  prevent  this  joint  from  “flipping." 

This  completes  the  forward  and  inverse  kinematics  solutions  for  the  Merlin  robot.  These  equa¬ 
tions  were  hand-optimized  and  coded  in  the  C  computer  programming  language.  The  full  (six 
DOF)  inverse  solution  can  be  calculated  in  less  than  1  ms  on  the  Compaq  computer. 


4  Conclusions 

With  the  changes  described  in  this  paper,  calculating  the  forward  kinematics  of  the  MBA  exoskele¬ 
ton  and  the  inverse  kinematics  of  the  Merlin,  plus  transferring  this  data  from  the  Compaq  to  the 
Merlin  takes  less  than  3  ms  (the  minimum  update  time  of  the  Merlin  robots  is  4  ms).  However,  the 


11 


FITTS  hardware  cannot  yet  operate  above  this  250  Hz  goal  due  to  the  32.8  kbaud  serial  (RS-422) 
link  between  the  MBA  exoskeleton  and  the  Compaq  computer. 

It  is  evident  we  will  achieve  maximum  throughput  for  this  system  by  increasing  the  bandwidth 
of  MBA-Compaq  communications.  Using  the  current  communications  protocol,  transferring  joint 
angle  data  from  the  exoskelet^n  to  the  Compaq  takes,  at  best,  8  ms  [2]  for  one  arm  of  the  exoskele¬ 
ton.  Some  of  this  delay  is  d  «,o  the  Compaq  computer  polling  the  port  too  often  during  data 
transfer,  essentially  preventing  data  flow  from  the  exoskeleton  to  the  computer  during  these  polls. 
This  problem  has  been  corrected  by  making  the  data  transfer  interrupt -driven. 

The  second  issue  of  concern  is  the  transfer  rate.  To  send  joint  angle  data  for  both  arms  of  the 
exoskeleton  in  less  than  1  ms,  the  exoskeleton  must  send  32  8-bit  bytes  (14  DOF  plus  grippers), 
in  addition  to  appropriate  start/stop  bits  and  handshaking  signals.  A  quick  calculation  shows  320 
kbaud  is  necessary  for  the  RS-422  link  to  support  this  throughput  requirement.  Another  approach 
is  to  send  the  data  in  parallel,  using  a  32  kbaud  transfer  rate.  Because  of  the  desire  to  be  able 
to  locate  the  exoskeleton  some  distance  from  the  control  computer,  the  differentially- driven  serial 
port  is  the  better  solution,  but  the  baud  rate  must  be  increased  above  320  kbaud.  Since  1  Mbaud 
is  often  achievable  with  RS-422  ports,  the  plan  is  to  implement  1  Af  fraud  on  this  system. 

Once  this  communications  upgrade  is  completed,  the  FITTS  hardware  will  be  able  to  perform 
at  its  peak  for  unilateral  teleoperation.  However,  several  issues  need  to  be  addressed  before  the 
system  is  ready  for  bilateral  teleoperation.  One  issue  is  deciding  which  controller  should  compute 
the  forward  kinematics  of  the  new  interfaces.  Another  is  determining  which  computer  should 
calculate  feedback  joint  torques  for  force  reflection,  using  the  well-known  relationship 

r  =  J 1  F  (66) 

where  r  is  the  (6  x  1]  vector  of  joint  torques,  J  1  is  the  transpose  of  the  Jacobian  of  the  bilateral 
interface,  and  F  is  the  (6  x  1]  vector  of  force/torque  information  [F,tFViFtiTJ%Tv%Tg]  from  the 
Merlin’s  end  effector. 

The  issues  listed  above,  and  others,  remain  to  be  answered  before  FITTS  becomes  the  modular 
testbed  of  bilateral  human  interfaces,  as  it  is  designed.  The  basic  plan  is  to  use  the  Compaq 
computer  to  control  the  Merlins,  and  as  the  center  for  data  exchange  between  masters  and  slaves. 
This  implies  that  the  bilateral  devices  must  compute  their  own  forward  kinematics,  outputting  only 
the  final  transformation  matrix  to  the  Compaq.  These  interfaces  must  also  be  prepared  to  accept 
6  DOF  force/torque  information  from  the  Compaq,  and  compute  the  appropriate  joint  torques  for 
force  feedback.  A  determining  factor  in  this  plan  is  to  keep  the  Merlin  update  rate  above  250  Hz. 

This  paper  showed  that  the  FITTS  testbed  has  been  established,  and  is  being  refined  at 
AAMRL.  This  testbed  will  soon  be  able  to  operate  for  unilateral  teleoperation,  comparing  var¬ 
ious  human  interface  devices  for  their  ease  of  operation.  A  preliminary  plan  has  also  been  pre¬ 
sented  which  will  allow  FITTS  to  become  a  modular  testbed  for  evaluating  the  various  bilateral 
human-interfaces  currently  used  as  input  devices  to  teleoperators. 
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